/* CPRE288 Lab8
 * 		@author: Andrew Hoyt & William Christensen
 * This program will move the Servo from full
 * left lock to full right lock. The angle of
 * the servo is displayed on the LCD in degrees.
 */
#include <stdio.h>
#include <avr/io.h>
#include "util.h"
#include "lcd.h"
#include "servo.h"
#include <math.h>
#include "various_functions.h"

// The following will not defined if defined in servo.h
#ifndef DEGREE
#define DEGREE 9
#endif

//#define FULLRIGHT 9774
//#define FULLLEFT 4369

#ifndef FULLRIGHT
#define FULLRIGHT 1059
#endif

#ifndef FULLLEFT
#define FULLLEFT 4537
#endif

const int pulseInterval = 43000;
/* From Prelab
 * Registers: TCCR3A,TCCR3B,TCCR3C,OCR3B
 * OCCR3B = 0xa7f8 (16bit register)
 * Prescaler of 8.
 * WGM = 0b1111
 */

void servoPos_init(servoPos *sp){ // Should work for fast pwm
	// Setup register and hardware
	DDRE |= 0x10; // Set port B as an output.
	TCCR3A = 0x23;
	TCCR3B = 0x1a; // Set prescaler, WGM3
//	TCCR3C = 0x00; // Force Output compare on OCCRB;

	TCCR3C = _BV(FOC3B);
	OCR3A = 43000-1; // Top output
	OCR3B = 2728;  // Center Value

	//Initialize structure values.
	sp->sStep = (FULLRIGHT - FULLLEFT)/180;
	sp->cPos=90;
}

/* Lab collected Data
 * Moves the servo with char (0-6)
 *
 * Robot 17:
 * Full right = 1006 | 45=1870
 * Full left = 4337  | 135=3474
 * Center = 2728
 *
 * Robot 15:
 * Full right = 1181 | 45=
 * Full left = 4633  | 135=
 * Center =
 *
 * Robot 1:
 * Full right = 1059
 * Full left = 4537
 */

/* void servoCal: Performs servo calibration
 * Input: struct pointer to servopos struct
 * that needs to be calibrated. The buttons work
 * just like in the lab, but scalled differently.
 * Press button 5 to program the specified value,
 * press button 6 to program the defaults.
 */
void servoPos_Cal(servoPos *sp){
	init_push_buttons();
	lcd_init();
	char inverted = 0;
	char buttonpress = 0;
	char endCalR = 0, endCalL=0;
	char message1[20];
	char message2[20];
	while(!(endCalR && endCalL)){
		lcd_home_line1();
		buttonpress = read_push_buttons();
		sprintf(message1,"%d",buttonpress);
		lcd_home_line4();
		lcd_puts(message1);
		
		//lcd_clear();
		if(endCalR) sprintf(message1,"Calibrate Left Pos");
		else sprintf(message1,"Calibrate Right Pos");
		sprintf(message2,"Value %i",OCR3B);
		lcd_home_line1();lcd_puts(message1);
		lcd_home_line2();lcd_puts(message2);

		switch(buttonpress){
		case 1:
			if(inverted) OCR3B-=1;
			else OCR3B+=1;
			break;
		case 2:
			if(inverted) OCR3B-=32;
			else OCR3B+=32;
			break;
		case 3:
			if(inverted) OCR3B-=128;
			else OCR3B+=128;
			break;
		case 4:
			if(inverted) inverted=0;
			else inverted = (char)1;
			break;
		case 5: // Program this value
			if(endCalR==0){
				sp->fullRight =(unsigned int) OCR3B;
				endCalR=1;
			}
			else if(endCalR==1 && endCalL==0){
				sp->fullLeft = (unsigned int) OCR3B;
				sp->sStep = (unsigned char) ((sp->fullLeft-sp->fullRight)/180);
				endCalL=1;
			}
			break;
		case 6:// Use preset value
			OCR3B = FULLRIGHT;wait_ms(800);sp->fullRight=(unsigned int)FULLRIGHT;
			OCR3B = FULLLEFT;wait_ms(800); sp->fullLeft=(unsigned int)FULLLEFT;
			sp->sStep = ((sp->fullLeft-sp->fullRight)/180);
			endCalR=1; endCalL=1;
			break;
		default:
			break;
		}
		wait_ms(100);
	}
}

/* Void Function, uses a wait timer to make sure
 * a move is complete before executing the next
 * move. Usually 10ms per degree shift.
 */
void servoPos_moveWait(int finish,servoPos *sp){
	sp->cPos = ((unsigned int)OCR3B-sp->fullRight)/sp->sStep;
	OCR3B = (finish*(sp->sStep)+sp->fullRight);
	wait_ms(abs((finish-sp->cPos)*10));
	sp->cPos = ((unsigned int)OCR3B-sp->fullRight)/sp->sStep;
}
